#include <stdio.h>
#include <stdint.h>
#include <wiringPi.h>
#include <wiringSerial.h>
#include <wiringPiI2C.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h> 
#include <string.h>
int fdI2C;
#define numpi 3.14159265358979324
static const double G0 = 9.7864;

/*BMI160 REG TABLE*/
#define BMI160_ADDR			0X68

#define BMI160_PMU_STATUS		0X0C

#define BMI160_GYRO_XOUTL_REG		0X0C	
#define BMI160_GYRO_XOUTH_REG		0X0D	
#define BMI160_GYRO_YOUTL_REG		0X0E	
#define BMI160_GYRO_YOUTH_REG		0X0F	
#define BMI160_GYRO_ZOUTL_REG		0X10	
#define BMI160_GYRO_ZOUTH_REG		0X11

#define BMI160_ACCEL_XOUTL_REG		0X12	
#define BMI160_ACCEL_XOUTH_REG		0X13	
#define BMI160_ACCEL_YOUTL_REG		0X14	
#define BMI160_ACCEL_YOUTH_REG		0X15	
#define BMI160_ACCEL_ZOUTL_REG		0X16	
#define BMI160_ACCEL_ZOUTH_REG		0X17
#define BMI160_FIFO_DATA		0X24

#define BMI160_ACC_CONF			0x40
/* rate 0B1000 100hz*/
#define BMI160_ACC_RANGE		0x41
/*  0B0011:+-2G RANGE;0b0101+-4g;0b1000:+-8g;0b1100:+-16g  */
#define BMI160_GYRO_CONF		0x42
#define BMI160_GYRO_RANGE		0x43

#define BMI160_FIFO_DOWNS		0x45
#define BMI160_FIFO_CONFIG1		0x46
#define BMI160_FIFO_CONFIG2		0x47

#define BMI160_FOC_CONF			0x69
#define BMI160_OFFSET_AX		0X71
#define BMI160_OFFSET_AY		0X72
#define BMI160_OFFSET_AZ		0X73
#define BMI160_OFFSET_GX		0X74
#define BMI160_OFFSET_GY		0X75
#define BMI160_OFFSET_GZ		0X76
#define BMI160_OFFSET_EN		0X77
#define BMI160_CMD			0x7E

#define BMI160CMD_START_FOC		0x03
#define BMI160CMD_ACCEL_NORMAL		0x11
#define BMI160CMD_GYRO_NORMAL		0x15
#define BMI160CMD_FIFO_FLUSH		0xB0
#define BMI160CMD_INT_RESET		0xB1
#define BMI160CMD_SOFTRESET		0xB6


typedef struct 
{
	int x;
	int y;
	int z;
}BMI160_Data;

typedef struct 
{
	int time;
	double x;
	double y;
	double z;
}BMI160_FloatData;

void initBMI160(void);
BMI160_Data BMI160_GetAccel(void);
BMI160_Data BMI160_GetGyro(void);
BMI160_Data GYRO_OFFSET;
void initBMI160()
{

    wiringPiSetup () ;
    fdI2C = wiringPiI2CSetup (BMI160_ADDR);  
    if (fdI2C >= 0)
    { // fd 为负数，说明IIC连接失败
        printf("fd = %d  res=%x\n",fdI2C,wiringPiI2CReadReg8(fdI2C,0x00));
	wiringPiI2CWriteReg8(fdI2C,BMI160_CMD,BMI160CMD_SOFTRESET);
	delay(100);
	wiringPiI2CWriteReg8(fdI2C,BMI160_FOC_CONF,0x40);
	wiringPiI2CWriteReg8(fdI2C,BMI160_OFFSET_EN,0x80+0x40);
	wiringPiI2CWriteReg8(fdI2C,BMI160_CMD,BMI160CMD_START_FOC);
	
	delay(200);
	/* range & rate setting */
        wiringPiI2CWriteReg8(fdI2C,BMI160_ACC_CONF,0x08);
	wiringPiI2CWriteReg8(fdI2C,BMI160_ACC_RANGE,0x03);
	wiringPiI2CWriteReg8(fdI2C,BMI160_GYRO_CONF,0x08); 
	wiringPiI2CWriteReg8(fdI2C,BMI160_GYRO_RANGE,0x00);
	wiringPiI2CWriteReg8(fdI2C,BMI160_GYRO_RANGE,0x00);
	/*FIFO setting*/
	/*wiringPiI2CWriteReg8(fd,BMI160_FIFO_DOWNS,0xff);
	wiringPiI2CWriteReg8(fd,BMI160_FIFO_CONFIG1,0x0f);
	wiringPiI2CWriteReg8(fd,BMI160_FIFO_CONFIG2,0xF0);*/
	/*IMU Begin*/
	wiringPiI2CWriteReg8(fdI2C,BMI160_CMD,BMI160CMD_ACCEL_NORMAL);
	delay(10);
	wiringPiI2CWriteReg8(fdI2C,BMI160_CMD,BMI160CMD_GYRO_NORMAL);
	delay(100);
	BMI160_Data gb,g;
	g.x=0;
	g.y=0;
	g.z=0;
	for(int i=0;i<100;i++)
	{
	  gb=BMI160_GetGyro();
	  g.x+=gb.x;
	  g.y+=gb.y;
	  g.z+=gb.z;
	}
	GYRO_OFFSET.x=g.x/100;
	GYRO_OFFSET.y=g.y/100;
	GYRO_OFFSET.z=g.z/100;
	printf("offset:%d,%d,%d",GYRO_OFFSET.x,GYRO_OFFSET.y,GYRO_OFFSET.z);
        printf("[ok]BMI160 Init\n");
    }
    else {
        printf("[Failed]BMI160 Init\n");
    }
}

BMI160_Data BMI160_GetAccel(void)
{
	static BMI160_Data temp;
	//wiringPiI2CWriteReg8(fd,BMI160_CMD,BMI160CMD_ACCEL_NORMAL);
	//delay(10);
	temp.x=(wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_XOUTH_REG)<<8);
	temp.x|=wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_XOUTL_REG);
	temp.y=(wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_YOUTH_REG)<<8);
	temp.y|=wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_YOUTL_REG);
	temp.z=(wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_ZOUTH_REG)<<8);
	temp.z|=wiringPiI2CReadReg8(fdI2C,BMI160_ACCEL_ZOUTL_REG);
	if(temp.x>0x7fff)
            temp.x = -(0xffff-temp.x);
	if(temp.y>0x7fff)
            temp.y = -(0xffff-temp.y);
	if(temp.z>0x7fff)
            temp.z = -(0xffff-temp.z);
	return temp;
}

BMI160_Data BMI160_GetGyro(void)
{
	static BMI160_Data temp;
	//wiringPiI2CWriteReg8(fd,BMI160_CMD,BMI160CMD_GYRO_NORMAL);
	//delay(50);		
	temp.x=(wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_XOUTH_REG)<<8);
	temp.x|=wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_XOUTL_REG);
	temp.y=(wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_YOUTH_REG)<<8);
	temp.y|=wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_YOUTL_REG);
	temp.z=(wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_ZOUTH_REG)<<8);
	temp.z|=wiringPiI2CReadReg8(fdI2C,BMI160_GYRO_ZOUTL_REG);
	if(temp.x>0x7fff)
            temp.x = -(0xffff-temp.x);
	if(temp.y>0x7fff)
            temp.y = -(0xffff-temp.y);
	if(temp.z>0x7fff)
            temp.z = -(0xffff-temp.z);
	temp.x-=GYRO_OFFSET.x;
	temp.y-=GYRO_OFFSET.y;
	temp.z-=GYRO_OFFSET.z;
	return temp;
}


int main()
{
  BMI160_Data gyro,accel;
  initBMI160();
  //wiringPiI2CReadReg8(fdI2C,)
  while(1)
  {
	 BMI160_FloatData gt,at; 
      gyro=BMI160_GetGyro();
      accel=BMI160_GetAccel();
      gt.x=(double)gyro.x*2000/32768*numpi/180.0;
      gt.y=(double)gyro.y*2000/32768*numpi/180.0;
	gt.z=(double)gyro.z*2000/32768*numpi/180.0;
	at.x=(double)accel.x*2*G0/32768.0;
	at.y=(double)accel.y*2*G0/32768.0;
	at.z=(double)accel.z*2*G0/32768.0;
      printf("g:%.4f,%.4f,%.4f\n",gt.x,gt.y,gt.z);
      printf("a:%.4f,%.4f,%.4f\n",at.x,at.y,at.z);
      delay(100);
  }
  return 0;
}
